#!/usr/bin/python3
# -*- coding:utf-8 -*-
import os
from numpy.core.multiarray import datetime_as_string
import serial
import cv2
import configparser
from datetime import datetime
import pickle
import time
import binascii
import ctypes
import socket
import numpy
from collections import Counter


import sys
sys.path.append("./")
sys.path.append("../")
#sys.path.append("/home/pi/CubeRobot_auto_doueyes/")
from vision.Pretreat import Pretreat
from vision.ClassifierVersion import Img2Status

def is_start(i):
    ser = serial.Serial('/dev/ttyS0', 115200)
    while True:
        size = ser.inWaiting()               # 获得缓冲区字符
        if size != 0:
            response = ser.read(size)# 读取内容并显示
            response = binascii.b2a_hex(response).decode("utf-8")
            print(response)
            if response == '4f':
                print("r")
                i += 1
                break
    return i

def rotate_finish(i):
    ser = serial.Serial('/dev/ttyS0', 115200)
    while True:
        size = ser.inWaiting()               # 获得缓冲区字符
        if size != 0:
            response = ser.read(size)# 读取内容并显示
            response = binascii.b2a_hex(response).decode("utf-8")
            if response == '4f':
                i += 1
                break
            # elif response == "50":
               # break
    return i

def trans(s):
    lis = []
    tran = ''
    for word in s.split():
        if len(word) == 1:
            word = '0' + word
            lis.append(word)
        else:
            lis.append(word)
    for element in lis:
        tran = tran + element + ' '
    tran = tran[:-1]
    return tran

def nothing():
    ser = serial.Serial("/dev/ttyS0", 115200)
    hex_str = bytes.fromhex('ff ff 01 56 56 00')
    ser.write(hex_str)
    ser.close()
def rotate_frame2():
    ser = serial.Serial("/dev/ttyS0", 115200)
    hex_str = bytes.fromhex('ff ff 03 59 6d ae 74 00')
    ser.write(hex_str)
    ser.close()

def rotate_frame1():
    ser = serial.Serial("/dev/ttyS0", 115200)
    hex_str = bytes.fromhex('ff ff 05 d2 e5 19 6d ae eb 00')
    ser.write(hex_str)
    ser.close()

def rotate_frame3():
    ser = serial.Serial("/dev/ttyS0", 115200)
    hex_str = bytes.fromhex('ff ff 03 d2 e5 16 cd 00')
    ser.write(hex_str)
    ser.close()

def rotate_backtostart():
    ser = serial.Serial("/dev/ttyS0", 115200)
    hex_str = bytes.fromhex('ff ff 07 59 6d a2 e5 19 6d ae 81 00')
    ser.write(hex_str)
    ser.close()

def rotate_fuyuan(move):
    ser = serial.Serial("/dev/ttyS0", 115200)
    hex_str = bytes.fromhex(move)
    ser.write(hex_str)
    ser.close()

def make_sure_open(cp, idx):
    __, frame = cp.read()
    if not __:
        __, frame = cp.read()
        print("waiting for cam {} open...".format(idx))

def getFinalResult(str_result):
    java_res = os.popen(
        "cd ..\n cd min2phase-dev-min\n java demo {0} | ../transform/build/transform"
        .format(str_result))
    lis = java_res.read()
    index_ff = lis.index('ff')
    finalRes = lis[index_ff:-7]+'0'
    return finalRes

def request_frame(sock):
    sock.send(b'##') #发送请求
    time.sleep(0.5)
    img1 = cv2.imread("server1.jpg")
    img2 = cv2.imread("server2.jpg")
    return  img1,img2

# ll = ctypes.cdll.LoadLibrary
# lib = ll("./libpycall.so")  #填入要调用的c文件
# lib.gpio_config()
# up_cam_cp = cv2.VideoCapture(0)

def main():
    config = configparser.ConfigParser()
    config.read("../configs/vision_pretreat.ini")
    pretreater = Pretreat(config)
    solver = Img2Status("../vision/ClfTrain/now.model")
    print('here')
    i = 0
    temp = 0
    i = is_start(i)
    print(i)

    sock = socket.socket()
    host = socket.gethostname()
    port = 12345
    sock.connect((host,port))
    print("connect!")

    pics = []
    frame1 = None
    frame2 = None
    start_time = datetime.now()
    print("start!") #测试加载配置模型需要多久

    while True:
            if i == 1:
                frame1,frame2 = request_frame(sock)
                pics.insert(0, frame2)
                print("get 0 image ok!")
                pics.insert(1, frame1)
                print("get 1 image ok!")
                print("--- Rotate Frame2 ---")
                rotate_frame2()
                i = rotate_finish(i)

            elif i == 2:
                print("--- get 2 image ---")
                time.sleep(0.3)
                frame1,frame2 = request_frame(sock)
                pics.insert(2, frame2)
                print("get 2 image ok!")
                pics.insert(3, frame1)
                print("get 3 image ok!")
                break
    print("start solve......")

    #pics[0] = cv2.imread("../configs/0.jpg")
    #pics[1] = cv2.imread("../configs/1.jpg")
    #pics[2] = cv2.imread("../configs/2.jpg")
    #pics[3] = cv2.imread("../configs/3.jpg")

    #pics[0] = cv2.imread("./tmp_image/0.jpg")
    #pics[1] = cv2.imread("./tmp_image/1.jpg")
    #pics[2] = cv2.imread("./tmp_image/2.jpg")
    #pics[3] = cv2.imread("./tmp_image/3.jpg")

    for i in range(4):
        cv2.imwrite("./image/%d.jpg" % (i), pics[i])

    print("End Rotate&cam Cost: {}ms".format(datetime.now() - start_time))
    ################
    # Process Part #
    ################

    # image process part
    print("image process start")
    start_time = datetime.now()
    color_vectors = pretreater.GetResult(pics)
    solver.GetResult(color_vectors)
    print("process color cost:\t", (datetime.now() - start_time))

    # min2phase part
    print("min2phase start")
    start_time = datetime.now()
    cube_status = solver.status
    with open("status.txt", "a+") as f:
        f.write(cube_status)
        f.write("\n")
    print(cube_status)
    
    print("U:",cube_status.count("U"),end=" ")
    print("D:",cube_status.count("D"),end=" ")
    print("R:",cube_status.count("R"),end=" ")
    print("L:",cube_status.count("L"),end=" ")
    print("F:",cube_status.count("F"),end=" ")
    print("B:",cube_status.count("B"))

    moves = getFinalResult(cube_status)
    print("step number:\t\t",int(moves.split()[2],16)*2)
    if int(moves.split()[2],16)*2 == 456:
        solver.ToPics(pretreater.perspectived_imgs)
        raise OSError
    #print(moves)
    #final_moves = trans(moves)
    #print(final_moves)
    #rotate_fuyuan(final_moves)
    print("min2phase cost:\t\t", (datetime.now() - start_time))
    rotate_finish(temp)
    print("total cost:\t\t",(datetime.now()-start_time))
    sock.close()
    # for i in range(4):
    #     cv2.imwrite("../configs/%d.jpg" % (i + 4), pics[i])
    solver.ToPics(pretreater.perspectived_imgs)


if __name__ == '__main__':
    while True:
        try:
            main()
        except OSError:
            nothing()  
